#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <string>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_simplefoc.h"


#define PHASE_U_GPIO 15
#define PHASE_V_GPIO 16
#define PHASE_W_GPIO 17
#define MOTOR_PP     7
#define MT6701_SPI_HOST SPI3_HOST
#define MT6701_SPI_SCLK_GPIO 2
#define MT6701_SPI_MISO_GPIO 1
#define MT6701_SPI_MOSI_GPIO -1
#define MT6701_SPI_CS_GPIO 42


BLDCMotor motor = BLDCMotor(MOTOR_PP);
BLDCDriver3PWM driver = BLDCDriver3PWM(PHASE_U_GPIO, PHASE_V_GPIO, PHASE_W_GPIO);
MT6701 mt6701 = MT6701(MT6701_SPI_HOST, (gpio_num_t)MT6701_SPI_SCLK_GPIO, (gpio_num_t)MT6701_SPI_MISO_GPIO, (gpio_num_t)MT6701_SPI_MOSI_GPIO, (gpio_num_t)MT6701_SPI_CS_GPIO);

float target_value = 0.0f;
Commander command = Commander(Serial);
void doTarget(char *cmd)
{
    command.scalar(&target_value, cmd);
}

extern "C" void app_main(void)
{
    SimpleFOCDebug::enable();                                        /*!< Enable debug */
    Serial.begin(115200);

//     mt6701.init();                                                  /*!< Enable as5600 */
//     motor.linkSensor(&mt6701);
//     driver.voltage_power_supply = 5;
//     driver.voltage_limit = 4;
// #ifdef USING_MCPWM
//     driver.init(0);
// #else
//     driver.init({1, 2, 3});
// #endif

//     motor.linkDriver(&driver);
//     motor.foc_modulation = SpaceVectorPWM;
//     motor.controller = MotionControlType::torque;

//     motor.PID_velocity.P = 4;
//     motor.PID_velocity.I = 0;
//     motor.PID_velocity.D = 0.04;
//     motor.PID_velocity.output_ramp = 10000;
//     motor.PID_velocity.limit = 10;

//     motor.useMonitoring(Serial);
//     motor.init();                                                    /*!< Initialize motor */
//     motor.initFOC();                                                 /*!<  Align sensor and start FOC */
//     command.add('T', doTarget, const_cast<char *>("target angle"));  /*!< Add serial command */
    driver.voltage_power_supply = 5;
    driver.voltage_limit = 4;

#ifdef USING_MCPWM
    driver.init(0);
#else
    driver.init({1, 2, 3});
#endif
    motor.linkDriver(&driver);

    motor.velocity_limit = 10;
    motor.voltage_limit = 5;
    motor.controller = MotionControlType::velocity_openloop;/*!< Set openloop control mode */

    while (1) {
        motor.loopFOC();
        motor.move(1.2f);
        command.run();
        vTaskDelay(1 / portTICK_PERIOD_MS);
    }
}
